/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.templates.LogitechController;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem;

/**
 *
 * @author matt
 */
public class AdjustArmPosition extends CommandBase {

    private static final double scale = 0.02;  // Note: sums at 50Hz
    private LogitechController controller;
    private LogitechController.AxisType axis;
    private double adjustmentDeadZone = 0.08;
    private boolean isAdjusting = false;


    public AdjustArmPosition() {
        System.out.println("armSubsystem = " + armSubsystem.getName());
        requires(armSubsystem);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
//        armSubsystem.initialize();
        controller = oi.getCoDriverController();
        axis = LogitechController.AxisType.kAxisLeftY;
    }

    // Called repeatedly (50 Hz) when this Command is scheduled to run
    protected void execute() {
        double adjustment = -controller.getRawAxis(axis.value);

        // if arm stick is in dead zone, set to current location
        if (Math.abs(adjustment) > adjustmentDeadZone) {
            adjustment = adjustment * scale;
            armSubsystem.adjustArm(adjustment);
            isAdjusting = true;
        } else {
            if (isAdjusting) {
                isAdjusting = false;
                armSubsystem.setPosition(armSubsystem.getPosition());
            }
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}
